#include "pid_pmeasure.h"
#include <sys/time.h>
#include <inttypes.h>

#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/task.h"
#include <math.h>
#include <time.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
static long millis(void)
{
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (((long)tv.tv_sec) * 1000) + (tv.tv_usec / 1000);
}
static void initialize(pid_pmeasure_ctrl_t *handle)
{
    handle->outputSum = *(handle->_output);
    handle->lastInput = *(handle->_input);
    if (handle->outputSum > handle->outMax)
        handle->outputSum = handle->outMax;
    else if (handle->outputSum < handle->outMin)
        handle->outputSum = handle->outMin;
}

pid_pmeasure_ctrl_t *pid_pmeasure_ctrl_new(float *input, float *output, float *setpoint,
                                           float kp, float ki, float kd, pid_measure_type pOn, pid_out_dir_type controllerDirection)
{
    pid_pmeasure_ctrl_t *handle = (pid_pmeasure_ctrl_t *)malloc(sizeof(pid_pmeasure_ctrl_t));

    handle->_output = output;
    handle->_input = input;
    handle->_setpoint = setpoint;
    handle->inAuto = false;

    pid_pmeasure_ctrl_set_output_limits(handle, 0, 255); // default output limit corresponds to
                                                         // the arduino pwm limits

    handle->SampleTime = 100; // default Controller Sample Time is 0.1 seconds

    pid_pmeasure_ctrl_set_controller_direction(handle, controllerDirection);
    pid_pmeasure_ctrl_set_tunings(handle, kp, ki, kd, pOn);

    handle->lastTime = millis() - handle->SampleTime;
    return handle;
}
void pid_pmeasure_ctrl_delete(pid_pmeasure_ctrl_t *handle)
{
    if (NULL != handle)
        free(handle);
}

void pid_pmeasure_ctrl_set_model(pid_pmeasure_ctrl_t *handle, pid_ctrl_model_type model)
{
    bool newAuto = (model == AUTOMATIC);
    if (newAuto && !handle->inAuto)
    { /*we just went from manual to auto*/
        initialize(handle);
    }
    handle->inAuto = newAuto;
}
bool pid_pmeasure_ctrl_compute(pid_pmeasure_ctrl_t *handle)
{
    if (!handle->inAuto)
        return false;
    unsigned long now = millis();
    unsigned long timeChange = (now - handle->lastTime);
    if (timeChange >= handle->SampleTime)
    {
        /*Compute all the working error variables*/
        double input = *(handle->_input);
        double error = *(handle->_setpoint) - input;
        double dInput = (input - handle->lastInput);
        handle->outputSum += (handle->ki * error);

        /*Add Proportional on Measurement, if P_ON_M is specified*/
        if (!handle->pOnE)
            handle->outputSum -= handle->kp * dInput;

        if (handle->outputSum > handle->outMax)
            handle->outputSum = handle->outMax;
        else if (handle->outputSum < handle->outMin)
            handle->outputSum = handle->outMin;

        /*Add Proportional on Error, if P_ON_E is specified*/
        double output;
        if (handle->pOnE)
            output = handle->kp * error;
        else
            output = 0;

        /*Compute Rest of PID Output*/
        output += handle->outputSum - handle->kd * dInput;

        if (output > handle->outMax)
            output = handle->outMax;
        else if (output < handle->outMin)
            output = handle->outMin;
        *(handle->_output) = output;

        /*Remember some variables for next time*/
        handle->lastInput = input;
        handle->lastTime = now;
        return true;
    }
    else
        return false;
}
void pid_pmeasure_ctrl_set_output_limits(pid_pmeasure_ctrl_t *handle, float Min, float Max)
{
    if (Min >= Max)
        return;
    handle->outMin = Min;
    handle->outMax = Max;

    if (handle->inAuto)
    {
        if (*(handle->_output) > handle->outMax)
            *(handle->_output) = handle->outMax;
        else if (*(handle->_output) < handle->outMin)
            *(handle->_output) = handle->outMin;

        if (handle->outputSum > handle->outMax)
            handle->outputSum = handle->outMax;
        else if (handle->outputSum < handle->outMin)
            handle->outputSum = handle->outMin;
    }
}
void pid_pmeasure_ctrl_set_tunings(pid_pmeasure_ctrl_t *handle, float Kp, float Ki, float Kd, pid_measure_type POn)
{
    if (Kp < 0 || Ki < 0 || Kd < 0)
        return;

    handle->pOn = POn;
    handle->pOnE = POn == P_ON_E;

    handle->dispKp = Kp;
    handle->dispKi = Ki;
    handle->dispKd = Kd;

    float SampleTimeInSec = ((float)handle->SampleTime) / 1000;
    handle->kp = Kp;
    handle->ki = Ki * SampleTimeInSec;
    handle->kd = Kd / SampleTimeInSec;

    if (handle->controllerDirection == REVERSE)
    {
        handle->kp = (0 - handle->kp);
        handle->ki = (0 - handle->ki);
        handle->kd = (0 - handle->kd);
    }
}
void pid_pmeasure_ctrl_set_controller_direction(pid_pmeasure_ctrl_t *handle, pid_out_dir_type dir)
{
    if (handle->inAuto && dir != handle->controllerDirection)
    {
        handle->kp = (0 - handle->kp);
        handle->ki = (0 - handle->ki);
        handle->kd = (0 - handle->kd);
    }
    handle->controllerDirection = dir;
}
void pid_pmeasure_ctrl_set_sampletime(pid_pmeasure_ctrl_t *handle, int NewSampleTime)
{
    if (NewSampleTime > 0)
    {
        float ratio = (float)NewSampleTime / (float)handle->SampleTime;
        handle->ki *= ratio;
        handle->kd /= ratio;
        handle->SampleTime = (unsigned long)NewSampleTime;
    }
}
float pid_pmeasure_ctrl_get_kp(pid_pmeasure_ctrl_t *handle)
{
    return handle->dispKp;
}
float pid_pmeasure_ctrl_get_ki(pid_pmeasure_ctrl_t *handle)
{
    return handle->dispKi;
}
float pid_pmeasure_ctrl_get_kd(pid_pmeasure_ctrl_t *handle)
{
    return handle->dispKd;
}

pid_ctrl_model_type pid_pmeasure_ctrl_get_model(pid_pmeasure_ctrl_t *handle)
{
    return handle->inAuto ? AUTOMATIC : MANUAL;
}
pid_out_dir_type pid_pmeasure_ctrl_get_direction(pid_pmeasure_ctrl_t *handle)
{
    return handle->controllerDirection;
}
